#!/bin/bash
PATH=/bin:/sbin:/usr/bin:/usr/sbin:/usr/local/bin:/usr/local/sbin:~/bin
export PATH
GOLO_Ver='0.3'
. /opt/openresty/nginx/scripts/golo.conf
source $ROS_PATH
source $ROS_GOLO_WS_PATH
export ROS_IP=$ROSIP
ecucost_teb=$(rosparam get /move_base/base_global_planner |grep test_planner |wc -l)
ecuswith=`rosparam get /ecu_swith`
ecucost_teb=$(rosparam get /move_base/base_global_planner |grep test_planner |wc -l)
if [ $ecucost_teb -ne 1 ];then
	rosrun dynamic_reconfigure dynparam load /move_base $GOLO_WS_PATH/golo/golo_nav/src/$teb &
fi
rosservice call /move_base/clear_costmaps
rosnode kill /scan_multi_merged
rosnode kill /null_scan_relay
roslaunch merged_percept scan_multi_merged.launch &
#rosrun dynamic_reconfigure dynparam load /move_base/TebLocalPlannerROS $GOLO_WS_PATH/golo/golo_nav/src/$ecucost &
init_times=1
while true
do
    rosSM=$(rosnode list|grep scan_multi_merged |wc -l)
    if [ $init_times -ge 25 ];then
	break
    fi
    ecucost_data=$(rosparam get /move_base/TebLocalPlannerROS/penalty_epsilon |grep 0.03 |wc -l)
    ecucost_teb=$(rosparam get /move_base/base_global_planner |grep test_planner |wc -l)  
    if [ $ecucost_teb -ne 1 ];then
	    rosrun dynamic_reconfigure dynparam load /move_base $GOLO_WS_PATH/golo/golo_nav/src/$teb &
    fi
    if [ $ecucost_teb -eq 1 ] && [ $ecucost_data -ne 1 ];then
	 rosrun dynamic_reconfigure dynparam load /move_base/TebLocalPlannerROS $GOLO_WS_PATH/golo/golo_nav/src/$ecucost &
    fi
    if [ $ecucost_teb -eq 1 ] && [ $ecucost_data -eq 1 ] && [ $rosSM == 1 ];then
        sleep 0.2
       rosparam set /ecu_swith false
       break
    fi
    init_times=$(($init_times+1))
    sleep 0.2
done

#if [ $rosSM == 1 ];then
#	sleep 0.5 && rosparam set /ecu_swith false	
#fi
#if [ $ecuswith != false ];then
# rosparam set /ecu_swith false
#fi




